﻿//#define MESSAGES

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Media;
using JigLibX.Collision;
using JigLibX.Physics;
using JigLibX.Geometry;
using JigLibX.Math;

namespace DARE
{
    /// <summary>
    /// The cube move one by one, and cannot move alone.
    /// His body is set to immovable when it stops.
    /// </summary>
    public class COneByOneController : APhysicController
    {

    #region identifiers

        public enum EDirection { NONE, UP, DOWN, LEFT, RIGHT }

    #endregion

    #region fields

        protected EDirection m_direction = EDirection.NONE;

        protected float m_tolerance = 0.1f;
        protected float m_detectground = 0.1f;
        protected float m_rayheight = 0.3f;
        protected Vector2 m_offset = new Vector2(0.5f, -0.5f);
        protected float m_maxSpeed = 2.0f;

        protected Vector3 m_desiredVelocity = Vector3.Zero;
        protected Vector2 m_destination = Vector2.Zero;
        protected Vector2 m_startpos = Vector2.Zero;
        protected Point m_dir = Point.Zero;
        protected bool m_isMoving = false;
        protected float m_bodyHeight = 1.0f;

        protected bool m_needUpdateDistanceFromGround = false;
        protected float m_distanceFromGround = 0.0f;

        protected const float m_delta = 0.01f; // used to avoid constant collisions

    #endregion
        
    #region properties

        public float DistanceFromGround
        {
            get
            {
                if (m_needUpdateDistanceFromGround)
                {
                    Vector3 velocity = m_body.Velocity;
                    Vector3 pos = m_body.Position;

                    m_body.MoveTo(pos + Vector3.UnitY * (m_bodyHeight / 2) + Vector3.UnitY * 0.1f, m_body.Orientation);
                    CRayCast.CIntersectInfo iInfo = m_node.Render3D.RayCast.IntersectFirst(pos, -Vector3.UnitY * 10.0f, 100);
                    m_body.MoveTo(pos, m_body.Orientation);
                    m_body.Velocity = velocity;
                    m_distanceFromGround = iInfo == null ? -1000 : pos.Y - iInfo.pos.Y - (m_bodyHeight / 2);
                    m_needUpdateDistanceFromGround = false;
                }
                return m_distanceFromGround;
            }
        }

        public bool IsOnTheGround
        {
            get { return DistanceFromGround != -1000 && DistanceFromGround <= m_detectground; }
        }

        public Vector2 Offset
        {
            get { return m_offset; }
            set { m_offset = value; }
        }

        public float Tolerance
        {
            get { return m_tolerance; }
            set { m_tolerance = value; }
        }

        public float DetectGround
        {
            get { return m_detectground; }
            set { m_detectground = value; }
        }

        public float RayHeight
        {
            get { return m_rayheight; }
            set { m_rayheight = value; }
        }

        public float MaxSpeed
        {
            get { return m_maxSpeed; }
            set { m_maxSpeed = value; }
        }

        public bool IsMoving
        {
            get { return m_isMoving; }
        }

    #endregion

    #region ctor

        public COneByOneController(CNode node) :
            base(node)
        { }

        public COneByOneController(CEntity entity) :
            base(entity.Node)
        { }

        protected override void Initialize()
        {
            m_body.SetBodyInvInertia(0, 0, 0);

            BoundingBox bb = m_node.Entity.Physic.Skin.WorldBoundingBox;
            m_bodyHeight = bb.Max.Y - bb.Min.Y;
            m_bodyHeight = 1; // yep, because the value is not exact otherwise... xD

            SetPosition((int)(m_body.Position.X + m_bodyHeight / 2.0f + m_tolerance) - m_bodyHeight / 2.0f, 
                (int)(m_body.Position.Z + m_bodyHeight / 2.0f + m_tolerance) - m_bodyHeight / 2.0f);
        }

    #endregion

    #region update

        public override void PreUpdate(float dt)
        { }

        public override void UpdateController(float dt)
        {
            if (m_isMoving)
            {
                float velY = m_body.Velocity.Y;

                m_desiredVelocity.X = m_maxSpeed * m_dir.X;
                m_desiredVelocity.Z = m_maxSpeed * m_dir.Y;

                m_body.Velocity = new Vector3(m_desiredVelocity.X, velY, m_desiredVelocity.Z);
            }
            else if (!IsOnTheGround && m_body.Immovable)
            {
// ===========================================================================================================================================
#if MESSAGES
                // can happens more than one time because of the rebound
                CDare.Instance.DebugMgr.DebugConsole.WriteLine("[PostUpdate] not on the ground and immovable, setting it to movable !");
#endif
// ===========================================================================================================================================
                m_body.Immovable = false;
            }
            m_needUpdateDistanceFromGround = true;
            base.UpdateController(dt);
        }

        public override void PostUpdate(float dt)
        {
            if (m_isMoving)
            {
                if (Vector2.DistanceSquared(new Vector2(m_body.Position.X, m_body.Position.Z), new Vector2(m_startpos.X, m_startpos.Y)) + 
                    m_tolerance >= 
                    Vector2.DistanceSquared(new Vector2(m_destination.X, m_destination.Y), new Vector2(m_startpos.X, m_startpos.Y)))
                {
                    m_body.MoveTo(new Vector3(m_destination.X, (int)(m_body.Position.Y + m_bodyHeight / 2.0f + m_tolerance) - m_bodyHeight / 2.0f + m_delta, m_destination.Y), Matrix.Identity);
                    m_body.Velocity = Vector3.Zero;
                    m_direction = EDirection.NONE;
                    m_body.Immovable = true;
                    m_isMoving = false;

// ===========================================================================================================================================
#if MESSAGES
                    CDare.Instance.DebugMgr.DebugConsole.WriteLine("[PostUpdate] new pos = " + m_body.Position.ToString());
                    CDare.Instance.DebugMgr.DebugConsole.WriteLine("[PostUpdate] setting body to immovable = true");
#endif
// ===========================================================================================================================================
                }
            }
            else if (IsOnTheGround && !m_body.Immovable)
            {
                m_body.MoveTo(new Vector3(m_destination.X, (int)(m_body.Position.Y + m_bodyHeight / 2.0f + m_tolerance) - m_bodyHeight / 2.0f + m_delta, m_destination.Y), Matrix.Identity);
                m_body.Velocity = Vector3.Zero;
                m_body.Immovable = true;

// ===========================================================================================================================================
#if MESSAGES
                // can happens more than one time because of the rebound
                CDare.Instance.DebugMgr.DebugConsole.WriteLine("[PostUpdate] distance from ground = " + m_distanceFromGround.ToString());
                CDare.Instance.DebugMgr.DebugConsole.WriteLine("[PostUpdate] new pos = " + m_body.Position.ToString());
                CDare.Instance.DebugMgr.DebugConsole.WriteLine("[PostUpdate] on the ground and movable, setting it to immovable !");
#endif
// ===========================================================================================================================================
            }
        }

    #endregion

    #region methods

        public void SetPosition(float x, float y)
        {
            SetPosition(new Vector2(x, y));
        }

        public void SetPosition(Vector3 position)
        {
            SetPosition(new Vector2(position.X, position.Z));
        }

        public void SetPosition(Vector2 position)
        {
            m_destination = position;
            m_startpos = position;
            m_direction = EDirection.NONE;
            m_body.MoveTo(new Vector3(position.X, m_body.Position.Y, position.Y), Matrix.Identity);
        }

        public void MoveTo(EDirection direction)
        {
            if (m_isMoving)
            {
// ===========================================================================================================================================
#if MESSAGES
                CDare.Instance.DebugMgr.DebugConsole.WriteLine("[MoveTo] cannot move because it's already moving !");
#endif
// ===========================================================================================================================================
                return;
            }
            if (!CanMove(direction))
            {
// ===========================================================================================================================================
#if MESSAGES
                CDare.Instance.DebugMgr.DebugConsole.WriteLine("[MoveTo] cannot move because an object block !");
#endif
// ===========================================================================================================================================
                return;
            }

            m_direction = direction;

            int x = (int)(m_body.Position.X - m_offset.X);
            int y = (int)(m_body.Position.Z - m_offset.Y);

            m_startpos = new Vector2(x + m_offset.X, y + m_offset.Y);
            m_dir = Point.Zero;

            switch (m_direction)
            {
                case EDirection.UP: m_dir.Y = -1; break;
                case EDirection.DOWN: m_dir.Y = 1; break;
                case EDirection.LEFT: m_dir.X = -1; break;
                case EDirection.RIGHT: m_dir.X = 1; break;
                default: break;
            }
            m_destination.X = x + m_dir.X;
            m_destination.Y = y + m_dir.Y;

            if ((int)m_destination.X != x || (int)m_destination.Y != y)
            {
                m_destination += m_offset;
// ===========================================================================================================================================
#if MESSAGES
                CDare.Instance.DebugMgr.DebugConsole.WriteLine("[MoveTo] setting body to immovable = false");
#endif
// ===========================================================================================================================================
                m_body.Immovable = false;
                m_isMoving = true;
            }
        }

        protected bool CanMove(EDirection direction)
        {
            Vector3 position = m_body.Position;
            Vector3 velocity = m_body.Velocity;
            Vector3 rayDirection = Vector3.Zero;

            switch (direction)
            {
                case EDirection.UP: rayDirection = -Vector3.UnitZ; break;
                case EDirection.DOWN: rayDirection = Vector3.UnitZ; break;
                case EDirection.LEFT: rayDirection = -Vector3.UnitX; break;
                case EDirection.RIGHT: rayDirection = Vector3.UnitX; break;
                default: return false;
            }

// ===========================================================================================================================================
#if MESSAGES
            CDare.Instance.DebugMgr.DebugConsole.WriteLine("\n[CanMove] direction = " + direction.ToString());
            CDare.Instance.DebugMgr.DebugConsole.WriteLine("[CanMove] cube pos = " + m_body.Position.ToString());
#endif
// ===========================================================================================================================================

            m_body.MoveTo(position + Vector3.UnitY * (m_bodyHeight + m_rayheight), m_body.Orientation);
            CRayCast.CIntersectInfo rayInfo = null;

            bool ableToMove = true;

            for (int i = -1; i <= 1; ++i)
            {
                Vector3 rayPos = position;

                if (rayDirection.Z != 0) rayPos.X += i / 2.0f;
                if (rayDirection.X != 0) rayPos.Z += i / 2.0f;

                rayPos.Y += m_rayheight;
                rayDirection.Y -= 0.01f;
                rayInfo = m_node.Render3D.RayCast.IntersectFirst(rayPos, rayDirection * 2, 100);
       
// ===========================================================================================================================================
#if MESSAGES
                CDare.Instance.DebugMgr.DebugConsole.WriteLine("[CanMove] ray pos = " + rayPos.ToString());
                if (rayInfo == null)
                    CDare.Instance.DebugMgr.DebugConsole.WriteLine("[CanMove] ray intersect nothing !");
                else
                {
                    Vector3 pos = rayInfo.pos;
                    float dist = Vector3.Distance(position, rayInfo.pos) - 0.5f;

                    CDare.Instance.DebugMgr.DebugConsole.WriteLine("[CanMove] object pos = " + pos.ToString() + ", object dist = " + dist.ToString());
                }
#endif
// ===========================================================================================================================================

                if (rayInfo != null && ((Vector3.Distance(position, rayInfo.pos) - 0.5f) < 1))
                {
                    ableToMove = false;
                    break;
                }
            }
            m_body.MoveTo(position, m_body.Orientation);
            m_body.Velocity = velocity;
            return ableToMove;
        }

    #endregion

    }
}
